/**************************************************************
*
* M3D_Math.H
*
* Basic mathematical operations
* Angle operations
**************************************************************/

//
// Global defines
//

__INLINE float m3dFSel (float cmp, float a, float b)
{
#if defined (_AP_XENON)
   return __fself(cmp, a, b);
#elif defined (_AP_PS3)
   return __fsel(cmp, a, b);
#else
   return cmp >= 0 ? a : b;
#endif
}


__INLINE float m3dFSign (float a)
{
#if (defined _AP_XENON || defined _AP_PS3)
   return m3dFSel(a, m3dFSel(-a, 0, 1), -1);
#else
   return a >= 0 ? ((-a) >= 0 ? 0.f : 1.f) : (-1.f);
#endif
}

__INLINE float m3dFMin(float a, float b)
{
#if (defined _AP_XENON || defined _AP_PS3)
	return m3dFSel((a)-(b),b,a);
#else
   return a < b ? a : b;
#endif
}

__INLINE float m3dFMax(float a, float b)
{
#if (defined _AP_XENON || defined _AP_PS3)
	return m3dFSel((a)-(b),a,b);
#else
   return a > b ? a : b;
#endif
}

//
// Angle transforms
//
#define m3dRad2Deg(rad)  ((rad)*180.f/M3D_PI)
#define m3dDeg2Rad(deg)  ((deg)*M3D_PI/180.f)


extern float   m3dAngleDegTo0_360    (float angle);
extern float   m3dAngleDegTo_180_180 (float angle);

//
// floating points comparisons, convertions and zero 
//

__INLINE float __CDECL m3dFAbs (float n) // returns the absolute value of float
{
#if !defined M3D_SUPPRESS_OPTIMIZATION
   #if defined(_AP_PC) || defined(_AP_XBOX) 
      *((int*)&n) &= 0x7FFFFFFF;
      return n;
   #elif defined(_AP_XENON)
      return __fabs(n);
   #elif defined(_AP_PS3)
	   return __builtin_fabsf(n);
   #endif
#else
   return n < 0 ? -n : n;
#endif
}

/*__INLINE float m3dFAbs (float x)
{
   m3dCheckValid(x);
   return fabsf(x);
}*/

__INLINE BOOL m3dIsZero_A (float x, float acc)
{
   return (m3dFAbs(x) < acc);
}

__INLINE BOOL m3dIsZero(float x) { return m3dIsZero_A(x, M3D_EPSILON); }

__INLINE bool m3dIsEqual_A (float x, float y, float acc)
{
   return m3dFAbs(y - x) < acc;
}

__INLINE bool m3dIsEqual(float x, float y) { return m3dIsEqual_A(x, y, M3D_EPSILON); }


__INLINE float m3dSign (float x)
{
   m3dCheckValid(x);
   
   if (x > 0) {
      return 1.f;
   } else if (x < 0) {
      return -1.f;
   }
   return 0.f;
}


__INLINE int m3dCompare_A (float x, float acc)
{
   m3dCheckValid(x);

   if (m3dIsZero_A(x, acc))
      return 0;
   else
      return x > 0.f ? 1 : -1;
}

__INLINE int m3dCompare (float x) { return m3dCompare_A(x, M3D_EPSILON); }


//
// Power functions
//
__INLINE float m3dPow2 (float x)
{ 
   m3dCheckValid(x);
   return x*x;
}

__INLINE float m3dPow3 (float x)
{ 
   m3dCheckValid(x);
   return x*x*x;
}

__INLINE float m3dPow4 (float x)
{ 
   m3dCheckValid(x);
   return m3dPow2(x*x);
}

__INLINE float m3dPow (float x, float p)
{
   m3dCheckValid(x);
   m3dCheckValid(p);
#if defined _AP_PS3 && !defined M3D_SUPPRESS_OPTIMIZATION && !defined(__SNC__)
   return __builtin_powf(x,p);
#else
   return powf(x,p);
#endif
}

// RW: BEGIN
__INLINE float m3dMod(float x, float y)
{
	return fmod(x, y);
}
// RW: END

__INLINE float m3dLogBase(float x, float base)
{
   return logf(x) / logf(base);
}

__INLINE float m3dLog2(float x)
{
   return m3dLogBase(x, 2.f);
}

//
// this might be transformed to table representation
//
#if defined _AP_PS3 && !defined M3D_SUPPRESS_OPTIMIZATION && !defined(__SNC__)
#define m3dCos(x)   (__builtin_cosf(x))
#define m3dSin(x)   (__builtin_sinf(x))
#define m3dTan(x)   (__builtin_tanf(x))
#else
#define m3dCos(x)   (cosf(x))
#define m3dSin(x)   (sinf(x))
#define m3dTan(x)   (tanf(x))
#endif
__INLINE void m3dSinCos(float x, float *s, float *c)
{
   *s = m3dSin(x);
   *c = m3dCos(x);
   return;
}

__INLINE float m3dACos (float x)
{
   ASSERT(x > -1.f-M3D_EPSILON && x < 1.f+M3D_EPSILON);
#if defined _AP_PS3 && !defined M3D_SUPPRESS_OPTIMIZATION && !defined(__SNC__)
   return __builtin_acosf(x);
#else
   return acosf(x);
#endif
}
__INLINE float m3dASin (float x)
{
   ASSERT(x > -1.f-M3D_EPSILON && x < 1.f+M3D_EPSILON);
#if defined _AP_PS3 && !defined M3D_SUPPRESS_OPTIMIZATION && !defined(__SNC__)
   return __builtin_asinf(x);
#else
   return asinf(x);
#endif
}
__INLINE float m3dATan (float x) 
{
#if defined _AP_PS3 && !defined M3D_SUPPRESS_OPTIMIZATION && !defined(__SNC__)
	return __builtin_atanf(x);
#else
   return atanf(x);
#endif
}
__INLINE float m3dATan2 (float y, float x) 
{
#if defined _AP_PS3 && !defined M3D_SUPPRESS_OPTIMIZATION && !defined(__SNC__) 
	return __builtin_atan2f(y,x);
#else
   return atan2f(y, x);
#endif
}

__INLINE float __CDECL m3dOOSqrt(float x)
{
   m3dCheckValid(x);
   ASSERT(x > M3D_EPSILON);

#if defined M3D_USE_SSE
   static const float three = 3.0f;
   static const float half  = 0.5f;

   __asm {
      movss       xmm3, x
      movss       xmm1, three
      movss       xmm2, half
      movaps      xmm4, xmm3
      rsqrtss     xmm3, xmm3
      mulss       xmm4, xmm3
      mulss       xmm2, xmm3
      mulss       xmm4, xmm3
      subss       xmm1, xmm4
      mulss       xmm2, xmm1
      movss       x, xmm2
   }
#elif defined (_AP_PS3) && !defined M3D_SUPPRESS_OPTIMIZATION && !defined(__SNC__)
	register float rOut;
	asm __volatile__("frsqrte %0, %1": "=f"(rOut) : "f"(x));
	return rOut;
#else
   return 1.0f / sqrtf(x);
#endif
}

__INLINE float m3dSqrt(float x)
{
   m3dCheckValid(x);
   ASSERT(x >= 0.0f);

#if defined M3D_USE_SSE
   // Use SSE intrinsics - allows compiler to inline and do register allocations in context
   // Also SSE sqrt is the fastest way to do this anyway
   float r;
   _mm_store_ss(&r, _mm_sqrt_ss(_mm_load_ss(&x)));
   return r;

#elif defined (_AP_PS3) && !defined M3D_SUPPRESS_OPTIMIZATION && !defined(__SNC__)
	return __builtin_sqrtf(x);
#else
   return sqrtf(x);
#endif
}

__INLINE float m3dSqrt3(float x)
{
   if (x < 0.f) {
      return -m3dPow(-(x), 1/3.f);
   }
   return m3dPow((x), 1/3.f);
}
//
// Standard timer decrementation
//
extern BOOL  m3dUpdateTimeField (float *timeField, float elapsedTime);

__INLINE void m3dCmpSwap(float *x1, float *x2) 
{
   float temp;

   if (*x1 > *x2) {
      temp = *x1;
      *x1 = *x2;
      *x2 = temp;
   }
   return;
}

template <class T>
__INLINE void m3dSwap(T &x1, T &x2) 
{
   T temp;

   temp = x1;
   x1 = x2;
   x2 = temp;
   return;
}

template <class T>
__INLINE void m3dCmpSwap(T &x1, T &x2) 
{
   T temp;

   if (x1 > x2) {
      temp = x1;
      x1 = x2;
      x2 = temp;
   }
   return;
}

__INLINE void m3dExpand_A(float *x1, float *x2, float acc) 
{
   if (m3dIsZero_A((*x2) - (*x1), acc)) {
      (*x1) -= acc;
      (*x2) += acc;
   }
   return;
}
#define m3dExpand(x1, x2)  m3dExpand_A(x1, x2, M3D_EPSILON) 

__INLINE void m3dCmpSwapExpand_A(float *x1, float *x2, float acc) 
{
   float temp;

   if (*x1 > *x2) {
      temp = *x1;
      *x1 = *x2;
      *x2 = temp;
   }
   if (((*x2) - (*x1)) < acc) {
      (*x1) -= acc;
      (*x2) += acc;
   }
   return;
}

#define m3dCmpSwapExpand(x1, x2) m3dCmpSwapExpand_A(x1, x2, M3D_EPSILON) 

//
// Linear interpolation functions (Lerp)
//
__INLINE float _m3dLerp (float x1, float x2, float x) 
{
   m3dCheckValid(x1);
   m3dCheckValid(x2);
   m3dCheckValid(x);
   return x1 + (x)*(x2 - x1);
}

//
// 'fa', 'fb' - line values at 'a' and 'b', 't' - parameter
//
__INLINE float m3dLerp (float fa, float fb, float a, float b, float t)
{
   m3dCheckValid(fa);
   m3dCheckValid(fb);
   m3dCheckValid(a);
   m3dCheckValid(b);
   m3dCheckValid(t);

#if (defined _AP_XENON || defined _AP_PS3) && !defined M3D_SUPPRESS_OPTIMIZATION
	// Branchless version
   double smaller_ab = fpmin(a, b);
   double dfa = __fsel(a-b, fb, fa);

   double bigger_ab = fpmax(a, b);
   double dfb = __fsel(a-b, fa, fb);

   double denom = bigger_ab - smaller_ab;
   double is_zero = denom - M3D_EPSILON;
   ASSERT(is_zero >= 0.0);
   double dt = __fsel(is_zero, t, smaller_ab);
   denom = __fsel(is_zero, denom, 1.0); 

   return (float)(dfa + (dfb - dfa)*(dt - smaller_ab) / (denom));
#else
   // otherwise Lerp won't work
   if (b < a) {
      m3dSwap(a, b);
      m3dSwap(fa, fb);
   }
   if (b-a < M3D_EPSILON) {
      //ASSERT(FALSE); // wrong assert
      return fa;
   }
   return fa + (fb - fa)*(t - a)/(b-a);
#endif
}

__INLINE float m3dLerpZero (float fa, float fb, float a, float b, float t)
{
   m3dCheckValid(fa);
   m3dCheckValid(fb);
   m3dCheckValid(a);
   m3dCheckValid(b);
   m3dCheckValid(t);

#if (defined _AP_XENON || defined _AP_PS3) && !defined M3D_SUPPRESS_OPTIMIZATION
	// Branchless version.  Branching inhibits inline.
   double if_zero = 0.5 * (fa + fb);

   // Copy-paste of m3dLerp, but without the assert. :/
   double smaller_ab = fpmin(a, b);
   double dfa = __fsel(a-b, fb, fa);

   double bigger_ab = fpmax(a, b);
   double dfb = __fsel(a-b, fa, fb);

   double denom = bigger_ab - smaller_ab;
   double is_zero = denom - M3D_EPSILON;
   double dt = __fsel(is_zero, t, smaller_ab);
   denom = __fsel(is_zero, denom, 1.0); 

   double if_not_zero = (dfa + (dfb - dfa)*(dt - smaller_ab) / (denom));

   return (float)__fsel(is_zero, if_not_zero, if_zero);
#else
   if (m3dIsZero(b-a)) {
      return 0.5f * (fa+fb);
   } else {
      return m3dLerp(fa, fb, a, b, t);
   }
#endif
}

//
// Clamping functions (Clamp)
//
__INLINE float m3dClamp (float fa, float fb, float a, float b, float t)
{
   m3dCheckValid(fa);
   m3dCheckValid(fb);
   m3dCheckValid(a);
   m3dCheckValid(b);
   m3dCheckValid(t);

#if (defined _AP_XENON || defined _AP_PS3) && !defined M3D_SUPPRESS_OPTIMIZATION
   // Branchless version.

	double smaller_ab = fpmin(a, b);
	double dfa = __fsel(a-b, fb, fa);
	double bigger_ab = fpmax(a, b);
	double dfb = __fsel(a-b, fa, fb);

	double dt = fpmax(t, smaller_ab);
	dt = fpmin(dt, bigger_ab);

	double denom = bigger_ab - smaller_ab;
	double is_zero = denom - M3D_EPSILON;
	//ASSERT(is_zero >= 0.0); // wrong assert

	dt = __fsel(is_zero, dt, smaller_ab);
	denom = __fsel(is_zero, denom, 1.0); 

	return (float)(dfa + (dfb - dfa)*(dt - smaller_ab) / denom);
#else
   if (b < a) {
      m3dSwap(a, b);
      m3dSwap(fa, fb);
   }

   //ASSERT(b - a > M3D_EPSILON); // wrong assert
   if (t <= a) {
      return fa;
   }
   if (t >= b) {
      return fb;
   }
   return m3dLerp(fa,fb,a,b,t);
#endif
}
__INLINE float m3dClampZero (float fa, float fb, float a, float b, float t)
{
   m3dCheckValid(fa);
   m3dCheckValid(fb);
   m3dCheckValid(a);
   m3dCheckValid(b);
   m3dCheckValid(t);
   // Enable for 360 at a more safe time.
#if /*defined _AP_XENON ||*/ defined _AP_PS3 && !defined M3D_SUPPRESS_OPTIMIZATION
   // if b < a, swap a,b and fa,fb.
   double smaller_ab = __fsel(b-a,a,b);
   double bigger_ab  = __fsel(b-a,b,a);
   double smaller_fafb = __fsel(b-a,fa,fb);
   double bigger_fafb =  __fsel(b-a,fb,fa);
   // Call lz
   double lz = m3dLerpZero(smaller_fafb,bigger_fafb,smaller_ab,bigger_ab,t);
   // if t < a, return fa
   lz = __fsel(t-smaller_ab,lz,smaller_fafb);
   // if b < t, return fb
   lz = __fsel(bigger_ab-t,lz,bigger_fafb);
   return lz;
#else
   if (b < a) {
      m3dSwap(a, b);
      m3dSwap(fa, fb);
   }
   if (t <= a) {
      return fa;
   }
   if (t >= b) {
      return fb;
   }
   return m3dLerpZero (fa,fb,a,b,t);
#endif
}

__INLINE float _m3dClamp (float a, float b, float t)
{
   m3dCheckValid(a);
   m3dCheckValid(b);
   m3dCheckValid(t);
#if (defined _AP_XENON || defined _AP_PS3) && !defined M3D_SUPPRESS_OPTIMIZATION
   double smaller_ab = fpmin(a, b);
   double bigger_ab  = fpmax(a, b);
   double dt = fpmax(t, smaller_ab);
   dt = fpmin(dt, bigger_ab);
   return (float)dt;
#else
   if (b < a) {
      m3dSwap(a, b);
   }
   if (t <= a) {
      return a;
   }
   if (t >= b) {
      return b;
   }
   return t;
#endif
}

//__INLINE float m3dSaturate(float x)
//{
//   return (float)fpmin(fpmax(x, 0.f), 1.f);
//}

//
// Smooth interpolation
//
//__INLINE float m3dSmoothStep(float x)
//{
//   ASSERT(-M3D_EPSILON < x && x < 1.f + M3D_EPSILON);
//   x = m3dSaturate(x);
//   return x*x*(3-2*x);
//}

//__INLINE float m3dSmoothLerp (float fa, float fb, float a, float b, float t)
//{
//   m3dCheckValid(fa);
//   m3dCheckValid(fb);
//   m3dCheckValid(a);
//   m3dCheckValid(b);
//   m3dCheckValid(t);
//
//   ASSERT(!m3dIsZero(b - a));
//   if (b < a) {
//      m3dSwap(a, b);
//      m3dSwap(fa, fb);
//   }
//   return fa + (fb - fa)*m3dSmoothStep((t - a)/(b-a));
//}

//__INLINE float m3dSmoothLerpZero (float fa, float fb, float a, float b, float t)
//{
//   ASSERT(!m3dIsZero(b - a));
//   if (m3dIsZero(b-a)) {
//      return 0.5f * (fa+fb);
//   } 
//   return m3dSmoothLerp(fa, fb, a, b, t);
//}

//__INLINE float m3dSmoothClamp (float fa, float fb, float a, float b, float t)
//{
//   if (b < a) {
//      m3dSwap(a, b);
//      m3dSwap(fa, fb);
//   }
//   ASSERT(b - a > M3D_EPSILON);
//   if (t < a) {
//      return fa;
//   }
//   if (t > b) {
//      return fb;
//   }
//   return m3dSmoothLerp(fa, fb, a, b, t);
//}
//__INLINE float m3dSmoothClampZero (float fa, float fb, float a, float b, float t)
//{
//   if (b < a) {
//      m3dSwap(a, b);
//      m3dSwap(fa, fb);
//   }
//   if (t < a) {
//      return fa;
//   }
//   if (t > b) {
//      return fb;
//   }
//   return m3dSmoothLerpZero(fa, fb, a, b, t);
//}

//
// Type-less clamping (works with other than 'float' types
//
#define _m3dClampT(a,b,t)  ((t) < (a)) ? (a): (((t) > (b))? (b):(t))


__INLINE float m3dFloor(float x, float step)
{
   float f;

   ASSERT(step >= 0);
   if (m3dIsZero(step)) {
      return x;
   } 
   f = (floorf(x/step)*step);
   return f;
}

__INLINE float m3dCeil (float x, float step)
{
   float c;

   ASSERT(step >= 0);
   if (m3dIsZero(step)) {
      c = x;
   } else {
      c = (ceilf(x/step)*step);
   }
   return c;
}

__INLINE float m3dRound (float x)
{
   if (x >= 0) {
      return floorf(x + 0.5f);
   } else {
      return ceilf(x - 0.5f);
   }
}

__INLINE float m3dRound (float x, float step)
{
   float r;

   ASSERT(step >= 0);
   if (m3dIsZero(step)) {
      r = x;
   } else if (x>0) {
      r = ((int)(x/step+0.5f))*step;
   } else if (x<0) {
      r = ((int)(x/step-0.5f))*step;
   } else { // x == 0
      r = 0.f;
   }

   return r;
}

#define m3dFraction(x, step) ((x) - m3dFloor(x, step))

__INLINE float m3dCycleToRange(float arg, float rangeLeft, float rangeRight)
{
   float ret = arg - m3dFloor(arg-rangeLeft, rangeRight+100.f*M3D_EPSILON-rangeLeft);
   ASSERT(ret >= rangeLeft-100.f*M3D_EPSILON && ret <= rangeRight+100.f*M3D_EPSILON);
   return ret;
}

extern int m3dSolveQuadraticEq(float a, float b, float c, float *root1, float *root2);
extern int m3dSolveCubicEq    (float a, float b, float c, float d, float *root1, float *root2, float *root3);

typedef float (* m3dFUNC)(float x, void *pData);

extern BOOL m3dFuncFindZero  (m3dFUNC f, void *pData, float a, float b, float eps, float *pA, float *pB);
extern void m3dFuncFindMin   (m3dFUNC f, void *pData, float a, float b, float eps, float *pA, float *pB);
extern BOOL m3dSolveLinSys2D (float a00, float a01, float a10, float a11, float b0, float b1, float *pX0, float *pX1);

extern BOOL  m3dIsPower2   (int x);
extern int m3dNearestGEPower2(int x);
extern float m3dSaw        (float period, float t);

//
// Integer math functions 
//
__INLINE int m3dIntDivCeil (int n, int d) 
{
   ASSERT(d != 0);
   return (n + (d-1)) / d;
}

__INLINE int m3dIntAlign (int n, int d) 
{
   STRONG_ASSERT(d > 0);
   return ((n - 1) / d + 1) * d;
}

__INLINE int m3dIntAlign2 (int n, int d) 
{
   STRONG_ASSERT(d > 0);
   STRONG_ASSERT(m3dIsPower2(d));
   return (n + (d-1)) & ~(d-1);
}

__INLINE int __CDECL m3dIntFloor (float x) 
{
   int res;

#if defined(_AP_PC) || defined(_AP_XBOX)
   WORD     cw, ccw;
   __asm {
      fstcw cw
   }
   ccw =  (cw & 0xf3ff) | 0x0400;  // truncate
   __asm {
      fldcw ccw
      fld      x
      fistp    res
      fldcw cw
   }
   ASSERT(m3dIsZero(res - floorf(x)));
   return res;
#else    // #if defined(_AP_PC) || defined(_AP_XBOX)
   res = (int)floor(x);
#endif   // #if defined(_AP_PC) || defined(_AP_XBOX)
   ASSERT(m3dFAbs(res-x) <= 1.f);
   return res;
}

__INLINE int m3dIntCeil (float x)
{
   return -m3dIntFloor(-x);
}

__INLINE int m3dIntRound (float x)
{
   int   r;

   if (x - floor(x) > 0.5) {
      r = m3dIntCeil(x);
   } else {
      r = m3dIntFloor(x);
   }
   return r;
}

extern BOOL m3dLimitChange_Float (float src, float dest, float maxLen, float *out);

__INLINE int __CDECL m3dFloat2Int(float n) // convert float to nearest int 
{
#if defined(_AP_PC) || defined(_AP_XBOX)
   int res;
   __asm {
      fld      n
      fistp    res
   }
   return res;
#elif defined (_AP_PS2)
   register int nOut = 0;

   asm __volatile__("
      cvt.w.s $f1, %1
      mfc1 %0, $f1
      ": "=r"(nOut) : "f"(n) : "$f1");
      return (nOut);
#else
   return (int)n;
#endif   // #if defined(_AP_PC) || defined(_AP_XBOX)
}

__INLINE float __CDECL m3dInt2Float(int n) // convert float to nearest int 
{         
  return (float)n;
}


/************************************
* m3dLog2P2
* log2 for power-2 numbers
************************************/
__INLINE int m3dLog2P2(int val)
{
   ASSERT(m3dIsPower2(val));
#ifdef _AP_XENON
   return 31 - _CountLeadingZeros(val);
#else
   int i = 0;
   while(val >>= 1) ++i;
   return i;
#endif
}


//
// End of file 'M3D_MATH.H'
//
